/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-24     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"

static void Icm42688Delay_ms(uint8_t ms)
{
    rt_thread_mdelay(ms);
}
static void Icm42688_R_reg(uint8_t addr, uint8_t *res, uint32_t num)
{
    uint8_t dat;

    dat =  addr | ICM42688_SPI_R;
    SPI_Handle.WthenRs(SPI_Handle.SPI1dev.ICM42688, &dat, 1, res, num);
}
static void Icm42688_W_reg(uint8_t addr, uint8_t val)
{
    uint8_t dat[2];
    dat[0] = addr | ICM42688_SPI_W;
    dat[1] = val;
    SPI_Handle.RWBytes(SPI_Handle.SPI1dev.ICM42688, dat, RT_NULL, 2);
}
static void Icm42688_self_check(void)
{
    uint8_t dat = 0;

    Icm42688_R_reg(ICM42688_WHO_AM_I, &dat, 1); //read IMU ID

    if(ICM42688_ID != dat)
    {
        rt_kprintf("icm42688_self-check failed ID:%x\n", dat);
    }
    else
    {
        return;
    }
}
static int Icm42688_init(void)
{
    Icm42688_self_check();
    Icm42688Delay_ms(1);

    /* acce scale: +-8g, freq: 1khz*/
    Icm42688_W_reg(ICM42688_ACCEL_CONFIG0,   0x01<<5);

    /* acce and gyro set to low noise mode, tempe is on by default */
    Icm42688_W_reg(ICM42688_PWR_MGMT0, 3<<0 | 3<<2);
    /* keep at least 45ms */
    Icm42688Delay_ms(100);

    return 0;
}

static uint8_t imu_rawData[14];
static Axis3i16 acce_raw_adc, gyro_raw_adc;
static float temper;

static void Get_icm42688RawData(void)
{
    Icm42688_R_reg(ICM42688_TEMP_DATA1, imu_rawData, 14);

    temper = ((int16_t)((uint16_t)imu_rawData[0]<<8 | imu_rawData[1]))/132.48f + 25;
    acce_raw_adc.x = (int16_t)((uint16_t)imu_rawData[2]<<8 | imu_rawData[3]);
    acce_raw_adc.y = (int16_t)((uint16_t)imu_rawData[4]<<8 | imu_rawData[5]);
    acce_raw_adc.z = (int16_t)((uint16_t)imu_rawData[6]<<8 | imu_rawData[7]);
    gyro_raw_adc.x = (int16_t)((uint16_t)imu_rawData[8]<<8 | imu_rawData[9]);
    gyro_raw_adc.y = (int16_t)((uint16_t)imu_rawData[10]<<8 | imu_rawData[11]);
    gyro_raw_adc.z = (int16_t)((uint16_t)imu_rawData[12]<<8 | imu_rawData[13]);
}

static int rt_icm42688_init(void)
{
    Icm42688_init();

    IMU_Handle.rawAcce_adc      = &acce_raw_adc;
    IMU_Handle.rawGyro_adc      = &gyro_raw_adc;
    IMU_Handle.getRawData       = Get_icm42688RawData;

    return RT_EOK;
}

INIT_DEVICE_EXPORT(rt_icm42688_init);
